// Astar.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "Astar.h"
#include <fstream>
#include <string>
#include <iostream>
#include <math.h>

#define CHOOSEMAP 0
namespace NBE
{

	void AstarApp::loadMaps()
	{
		//load all the txt map

		wchar_t* mapsname[TotalMaps] = {L"brutal.txt",L"brutal2.txt",L"H-weight_test.txt",L"newtest.txt",L"notsobrutal.txt",L"sashank test.txt",L"very-brutal.txt",L"veryhard.txt",L"sample.txt"};

		for (int i = 0;i<TotalMaps;++i)
		{
			//if(i != TotalMaps-1)continue;

			std::ifstream* ifs = new std::ifstream(L"tests\\" + std::wstring(mapsname[i]));		
			char buffer[Map_MaxWidth];
			ifs->getline(buffer,Map_MaxWidth);
			uint w = strlen(buffer) ;// n
			uint h = 0;
			AstarMap* oneMap = new AstarMap(std::wstring(mapsname[i]));

			for (;;++h,ifs->getline(buffer,Map_MaxWidth))
			{
				for (uint j =0; j< w; ++j)
				{
					switch ( buffer[j] )
					{
					case 'S':
						oneMap->nodesList.push_back(AstarNode(START,j,h,w*h+j));
						oneMap->startNode = oneMap->nodesList.size()-1;
						break;
					case 'E':
						oneMap->nodesList.push_back(AstarNode(END,j,h,w*h+j));
						oneMap->endNode = oneMap->nodesList.size()-1;
						break;
					case '.':
						oneMap->nodesList.push_back(AstarNode(EMPTY,j,h,w*h+j));
						break;
					case '/':
						oneMap->nodesList.push_back(AstarNode(SEMIBLOCKER,j,h,w*h+j));
						break;
					case '#':
						oneMap->nodesList.push_back(AstarNode(BLOCKER,j,h,w*h+j));
						break;
					default:
						std::cout<<"Map error: can't recognize the character or this line doesn't match the width\n";
						break;
					}
				}

				if(ifs->eof())
				{
					ifs->close();
					delete ifs;
					break;
				}

			}



			oneMap->width = w;
			oneMap->height = h+1;


			std::wcout<<L"map:"<<  (mapsname[i]) <<L" loaded\n";
			m_maps.push_back(oneMap);
		}

	}

	AstarApp::~AstarApp()
	{
		//maps
		for (auto it = m_maps.begin();it!=m_maps.end();)
		{
			delete *it;
			it = m_maps.erase(it);
		}

	}

	void AstarApp::processing()
	{
		std::wcout<<L"--------------------------\n";

		// Astar algorithm
		for (uint i=0;i<m_maps.size();++i)
		{
			uint s = m_maps[i]->startNode;
			uint e = m_maps[i]->endNode;

			std::vector<uint> openList,closedList;

			m_maps[i]->nodesList[s].nodeType = OPEN;
			openList.push_back(s);
			//start!
			doPathFinding(s,e,m_maps[i],openList,closedList);
		}
		
		system("pause");
		
	}


	bool AstarApp::doPathFinding(uint current, uint e,AstarMap* pMap,
									std::vector<uint>& open,std::vector<uint>& closed)
	{
		while(current != e )
		{
			AstarNode& curNd = pMap->nodesList[current];
			curNd.nodeType = CURNODE;
			//printMapOnConsole(pMap);
			addNeighorsToOpenList(current,pMap,open);
			//printMapOnConsole(pMap);
			removeNodeInOpenList(current ,open);
			curNd.nodeType = CLOSED;
			closed.push_back(current);
			//printMapOnConsole(pMap);
			if(open.size() > 0)
				current = getLowestCostNodeInOpenList(open,pMap);
			else
			{
				std::cout<<"open list empty!\n";
				//printMapOnConsole(pMap);
			}
			
		}

		uint back = e;
		for (;back != pMap->startNode;)
		{
			pMap->nodesList[back].nodeType = PATH;
			back = pMap->nodesList[back].parent;
		}
		printOutMap(pMap);
		//printMapOnConsole(pMap);
		return true;
	}

	uint AstarApp::getLowestCostNodeInOpenList(std::vector<uint>& open,AstarMap* pMap)
	{
		uint rt = open.back();
		AstarNode backnd = pMap->nodesList[rt];
		uint val = backnd.G + backnd.H;
		for(uint i = 0;i < open.size() ;++i)
		{
			AstarNode nd  = pMap->nodesList[open[i]];
			if(val > nd.G + nd.H)
			{
				val = nd.G + nd.H;
				rt = open[i];
			}
			else if(val == nd.G + nd.H)
			{
				if(backnd.H > nd.H)
				{
					val = nd.G + nd.H;
					rt = open[i];
				}
			}
		}
		return rt;
	}

	void AstarApp::printOutMap(AstarMap* pMap)
	{
		//system("CLS");
		std::wcout<<pMap->mapName<<L" done!\n";
		std::ofstream ofs(L"solution_" + pMap->mapName ,std::ofstream::binary);
		
		for (uint idx = 0; idx < pMap->nodesList.size(); ++idx)
		{
			//std::cout<<m_mapSymbol[ pMap->nodesList[idx].nodeType ];
			ofs<<m_mapSymbol[ pMap->nodesList[idx].nodeType ];
			if((idx+1)%pMap->width == 0)
			{
				ofs<<"\n";
				//std::cout<<"\n";
			}
		}
		ofs.close();
		
		
	}

	void AstarApp::printMapOnConsole(AstarMap* pMap)
	{
		system("CLS");
		for (uint idx = 0; idx < pMap->nodesList.size(); ++idx)
		{
			std::cout<<m_mapSymbol[ pMap->nodesList[idx].nodeType ];
			 
			if((idx+1)%pMap->width == 0)
			{
				 
				std::cout<<"\n";
			}
		} 


	}
	void AstarApp::addNeighorsToOpenList(uint nd_idx,AstarMap* pMap,std::vector<uint>& open)
	{
		//uint end = pMap->endNode;
		AstarNode& nd = pMap->nodesList[nd_idx];

		auto checkNewNode = [&](AstarNode& newNode){
			if (newNode.nodeType == OPEN)
			{
				if( nd.G + 1 < newNode.G)
				{
					//re-connect the node
					newNode.G = nd.G + 1;
					newNode.parent = nd.idx;
				}
			}
			else if(newNode.nodeType != CLOSED && newNode.nodeType != BLOCKER)
			{
				newNode.parent = nd.idx;
			
				newNode.G = nd.G + 1;
				
				if(newNode.nodeType==SEMIBLOCKER)
				{
					newNode.H = calToEnd(newNode,pMap) - 1 + 1 * 5 ;
				}
				else
				{
					newNode.H = calToEnd(newNode,pMap)  ;
				}
				newNode.nodeType = OPEN;
				open.push_back( newNode.idx ); 
			}
		};
		

		if((uint)nd.z + 1 < pMap->height)
		{
			AstarNode& newNode = pMap->getNode(nd.x,nd.z + 1);
			checkNewNode(newNode);
		}

		if(nd.z - 1 >= 0)
		{
			AstarNode& newNode = pMap->getNode(nd.x,nd.z - 1);
			checkNewNode(newNode);
		}

		if((uint)nd.x + 1 < pMap->width)
		{
			AstarNode& newNode = pMap->getNode(nd.x + 1,nd.z);
			checkNewNode(newNode);
		}

		if(nd.x - 1 >= 0)
		{
			AstarNode& newNode = pMap->getNode(nd.x - 1,nd.z);
			checkNewNode(newNode);
		}




		
		
		
		
	}


	void AstarApp::removeNodeInOpenList(uint idx, std::vector<uint>& open)
	{
		for(auto it = open.begin();it!=open.end();++it)
		{
			if (*it == idx)
			{
				open.erase(it);
				return;
			}
		}
		std::cout<<"!!!!!!!!! current node is not in the open list!\n";
		system("pause");

	}

	uint AstarApp::calToEnd(AstarNode& nd,AstarMap* pMap)
	{
		AstarNode& endNd = pMap->nodesList[pMap->endNode];
		return (endNd.x > nd.x ? endNd.x - nd.x: nd.x - endNd.x ) + (endNd.z > nd.z ? endNd.z - nd.z : nd.z - endNd.z);
	}

}


using namespace NBE;

int _tmain(int argc, _TCHAR* argv[])
{
	_CrtSetDbgFlag( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF );
	_CrtSetReportMode( _CRT_ERROR, _CRTDBG_MODE_DEBUG );


	AstarApp app;
	app.loadMaps();
	app.processing();
	return 0;
}

